package org.team751.subsystems;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.command.Subsystem;
import org.team751.RobotMap;
import org.team751.commands.DriveWithJoystick;

/**
 * Subsystem for the drivetrain<br />
 * All commands must implement the MotorSafety thing.<br />
 * This means that they must call Drivetrain::stop() when they complete.
 * @author Sam Crow
 */
public class Drivetrain extends Subsystem {
    protected RobotDrive drive;
    
    public Drivetrain(){
        drive = new RobotDrive(RobotMap.driveLeft, RobotMap.driveRight);
        drive.setSafetyEnabled(false);
    }
    
    // Put methods for controlling this subsystem
    // here. Call these from Commands.

    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        setDefaultCommand(new DriveWithJoystick());
    }
    
    public void operatorDrive(Joystick joystick){
        drive.setSafetyEnabled(true);
        drive.arcadeDrive(joystick);
    }
    
    public void stop(){
        drive.drive(0, 0);
        drive.setSafetyEnabled(false);
    }
}
